package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO.EnhancedIOException;

public class EnhancedIO {
    private DriverStationEnhancedIO enhancedIO;
    
    public EnhancedIO() {
        this.enhancedIO = DriverStation.getInstance().getEnhancedIO();
    }

    public void setIOState(int channel, boolean value) {
        try
        {
            enhancedIO.setDigitalOutput(channel,value); 
        }
        catch (EnhancedIOException error)
        {
            System.out.println("BLAW: " + error);
        }
    }
    
    public boolean getIOState(int channel) {
        try
        {
            return enhancedIO.getDigital(channel);
        }
        catch (EnhancedIOException err)
        {
            System.out.println("BLAW: " + err);
            return false;
        }
    }
    
    public void setBallSensorStates(boolean low, boolean mid, boolean high) {
        try {
            enhancedIO.setDigitalOutput(1,low);
            enhancedIO.setDigitalOutput(2,mid);
            enhancedIO.setDigitalOutput(3,high);
        }
        catch (EnhancedIOException err)
        {
            System.out.println("BLAW: " + err);
        }
    }
    
    public void setRampArmLightState(boolean state) {
        try {
            enhancedIO.setDigitalOutput(4,state);
        }
        catch (EnhancedIOException err)
        {
            System.out.println("BLAW: " + err);
        }
    }
    
    public void setManualModeLightState(boolean state) {
        try {
            enhancedIO.setDigitalOutput(5,state);
        }
        catch (EnhancedIOException err)
        {
            System.out.println("BLAW: " + err);
        }
    }
    
    public void setTargetAcquiredLightState(boolean state) {
        try {
            enhancedIO.setDigitalOutput(6,state);
        }
        catch (EnhancedIOException err)
        {
            System.out.println("BLAW: " + err);
        }
    }
}
